module Zb_physics
use etienne_gino
use build_lattice
use gauss_dis
implicit none
  public
PRIVATE track_RING
real(dp) cs(3)
real(dp) rcs(3)
real(dp) total_tunes(3)
integer, parameter :: nbmp_total=10001
real(dp) bmp_data(6,0:nbmp_total),init_bpm(6)
logical :: fill_orbit=my_false
  INTERFACE track_standard
     MODULE PROCEDURE track_RING ! not private
  END INTERFACE
real :: first_fib=1.0, second_fib=1.e6
INTEGER :: my_colour 
real(dp) mur(2),dmur(3),jsol(2),A_str,phi_str,a_str2,phi_str2
integer :: sum_res_val=1
integer imur(2),idmur(3),ijsol(2),iA_str,iphi_str,isum_res_val,ij_perp,iI_0,ipsi_0
real(dp) :: J_perp=0.d0, dt_sum_res=1.d-4,j_scale=1.d6
real(dp) I_0,PSI_0
real(dp) a_res(4,4),a_resi(4,4)
logical :: stop_track=.false.
contains 





subroutine track_RING(r,flag_layout,flag_a,flag_map,turns)
implicit none
type(graph_data) r
integer i,J,k,ND2,NPARA,ma,nt,turns,ik,int_n,node1,node2,m
logical flag_a,flag_map
integer flag_layout,m_orbit_start
type(fibre), pointer :: c
type(damap) ai,mapp,inva
type(tree) mt,mappt,invat
real(dp) ray(lnv),rayt(lnv),tunes0(3),dtunes(3),tunes(3),omega0(3),r0b(3),r0a(3)
real(dp) rayi(lnv)
real xi,xf,posr,pha(3),phar,iperp,v
type(three_d_info) v3d
type(integration_node),pointer :: intnode,tin
logical testposr
logical doplot
type(work) werk
 call gmSetWidgetStatus(but1, GunSELECTABLE)
 call gmSetWidgetStatus(but2, GunSELECTABLE)
 
r%turn_tracked=0
!call RESET_APERTURE_FLAG
doplot=.false.

    if(orbit_track) then
    call Locate_orbit_start(my_start,m_orbit_start)
    endif

nullify(c)
write(6,200)" Ray Input   ",r%r
if(random_colour) then
    IF(KEEP_COLOUR) THEN
	  KEEP_COLOUR=.FALSE.
	 ELSE

 	 909 v=ABS(RANF()*360.0)
     if(v>360.0) goto 909
     call gDefineHLS(colour_random, v, 0.5, 1.0)
     ENDIF
my_colour=colour_random
else
my_colour=r%colour
	  KEEP_COLOUR=.FALSE.
endif

200 FORMAT(A13,6(1X,g21.14))

if(flag_a.and.flag_map) return

DO I=1,R%ND
IF(.NOT.R%IN(I)) RETURN
ENDDO 

if(flag_a) then
   dtunes=0.0_dp
   tunes=0.0_dp
   tunes0=0.0_dp
   total_tunes=0.0_dp
   c_%check_da=.true.
   if(.not.c_%stable_da) write(6,*) 1000
   CALL INIT(MY_eSTATE,my_order,MY_NP,BERZ,ND2,NPARA)
   if(.not.c_%stable_da) write(6,*) 2000
   ma=newfile
   open(unit=ma,file="a_inv.txt")
   call alloc(ai);call alloc(mt)
   call alloc(inva);call alloc(invat)
   call read(ai,ma)
   call read(inva,ma)
   ma=closefile
   mt=ai
   invat=inva
   ray=0.d0
   cs=0.0_dp
   rcs=0.0_dp
   if(.not.c_%stable_da) write(6,*) 3000
endif

if(flag_map) then
  c_%check_da=.true.
  CALL INIT(my_estate,my_order,MY_NP,BERZ,ND2,NPARA)
  ma=newfile
  open(unit=ma,file="only_map.txt")
  call alloc(mapp);call alloc(mappt)
  call read(mapp,ma)
  ma=closefile
  mappt=mapp
  rayt=0.d0
endif


call move_to( my_ering,c,my_start )


if(r%nd<3.and.(.not.around_fix)) then
  r%r(5)=my_delta; r%r(6)=0.d0;
endif
if(r%nd<2) then
  r%r(3)=0.d0; r%r(4)=0.d0;
endif


if(r%do_a_for_dyn) call fill_ray(r)

if(around_fix) then
 write(6,*) "around fix"
 if(r%nd==3) then
  do i=1,6
   r%r(i)=r%r(i)+my_fix(i)
  enddo
 elseif(r%nd==2) then
  do i=1,4
   r%r(i)=r%r(i)+my_fix(i)
  enddo
   r%r(5)=my_fix(5); r%r(6)=my_fix(6)
 else
  do i=1,2
   r%r(i)=r%r(i)+my_fix(i)
  enddo
   r%r(5)=my_fix(5); r%r(6)=my_fix(6)
   r%r(3)=my_fix(3); r%r(4)=my_fix(4)
 endif
endif
xi=r%r(1);xf=xi

if(flag_map) rayt(1:6)=r%r(1:6)

write(6,200)" Ray Tracked ",r%r

if(flag_layout==1) THEN   ! SETTING WORM
 if(.not.associated(my_ering%t))then
  write(6,*) "Making a node layout "
  call MAKE_NODE_LAYOUT(my_ering)
  call FILL_SURVEY_DATA_IN_NODE_LAYOUT(my_ering)
 endif

 call alloc_three_d_info(v3d)
 v3d%reference_ray=my_fix
 v3d%x=r%r
 v3d%scale=scale_info(0)
ENDIF
 ! Write(6,*) " Starts at ", MY_START

if(r%tc/=0) then
    call move_to_INTEGRATION_NODE(my_ering%t,intnode,r%tc)    
endif

do i=0,r%nturns
  if(orbit_track.and.(i==0.or.i==r%nturns)) then
   werk=my_ering%start
   if(accelerate.or.ramp) write(6,*) " time = ",x_orbit_sync(6)/clight ," seconds"
   if(my_ORBIT_LATTICE%STATE%modulation) write(6,*) " time = ",xsmt/clight," seconds"
   write(6,*) "Design kinetic energy = ",werk%kinetic
  endif



if(i/=0) then

select case(flag_layout)
CASE(1)

 if(r%tc==0) then
  intnode=>c%t1
  node1=c%t1%pos
  node2=c%t1%pos+my_ering%t%n-1
   else
  node1=r%tc
  node2=r%tc+my_ering%t%n-1
 endif
 if(associated(p_gino_1)) then
  call Universe_max_node_n(int_n)
  node1=1
  tin=>my_ering%start%t1
  do k=1,int_n
    if(associated(tin,p_gino_1%t1)) exit
    tin=>tin%next
    node1=node1+1
  enddo
   write(6,*) "node1 ",node1
   write(6,*) "name ",tin%parent_fibre%mag%name
   write(6,*) tin%parent_fibre%parent_layout%name
     intnode=>tin
 endif
 if(associated(p_gino_2)) then
  call Universe_max_node_n(int_n)
  node2=1
  tin=>my_ering%start%t1
  do k=1,int_n
    if(associated(tin,p_gino_2%t1)) exit
    tin=>tin%next
    node2=node2+1
  enddo
   write(6,*) "node1 ",node1
   write(6,*) "name ",intnode%parent_fibre%mag%name
   write(6,*) intnode%parent_fibre%parent_layout%name
   write(6,*) "node2 ",node2
   write(6,*) "name ",tin%parent_fibre%mag%name
   write(6,*) tin%parent_fibre%parent_layout%name
 endif

 do k=node1, node2

  xf=V3d%x(1)
  call TRACK_node_v( intnode,V3d,my_estate,ref=my_true)
  if(.not.check_stable) then
    call check_ptc_exceptions(r,my_ering,my_lost_position)

     write(6,200)" Ray Graphed ",r%s 
     write(6,200)" Ray Tracked ",r%r

     write(6,*) " Tracking done ",r%turn_tracked
   return
  endif
  if(associated(intnode%bb)) then
   call draw_bb(intnode,plot(4))
  endif
!  call TRACK_x( my_ering,V3d,my_estate,POS1=k,POS2=k+1)

  posr= float(intnode%parent_fibre%pos) !-1.0
  if(intnode%cas==case2.or.intnode%cas==casep2)posr=posr+1.0
  if(intnode%cas==0)posr=posr+float(intnode%pos_in_fibre-2)/intnode%parent_fibre%mag%p%nst
  
  if(second_fib<first_fib) then
    testposr=posr>=first_fib.or.posr<=second_fib &
     .and.intnode%parent_fibre%pos/=int(first_fib)
 else
    testposr=posr>=first_fib.and.posr<=second_fib.and.intnode%parent_fibre%pos/=int(first_fib)
 endif

 if(testposr) then
  
  if(intnode%parent_fibre%mag%kind/=1111) then !kind0) then  !.and.intnode%parent_fibre%mag%kind/=kind1) then
   
   if(intnode%cas==CASE0.OR.intnode%cas==CASET)   then
!2      if(intnode%previous%cas==CASE1) r0b=V3d%r0  ! drift 
!2      if(intnode%next%cas==CASE2) r0a=V3d%r       ! drift 
!        if(intnode%parent_fibre%mag%p%b0/=0.d0) then
!          if(intnode%parent_fibre%mag%bn(1)==0.d0) goto 1066
!        endif
!         if(intnode%parent_fibre%mag%kind/=kind1)
IF(intnode%parent_fibre%mag%PLOT) call e_gm_draw_ray_node(my_colour,plot(4),V3d)
!         1066 continue
!        if(intnode%previous%cas==CASE1.and.intnode%parent_fibre%mag%kind==kind1) then      ! drift 
!2        if(intnode%previous%cas==CASE1) then      ! drift 
!          V3d%r0=r0a      ! drift 
!          V3d%r=r0b       ! drift 
      !    if(doplot) call e_gm_draw_ray_node(r%colour,plot(4),V3d)   ! drift 
!2            call e_gm_draw_ray_node(my_colour,plot(4),V3d)
!2            doplot=.true.   ! drift 

!2        endif    ! drift 
   endif

  endif
 
 9876 continue
 endif

   r%r=V3d%x
   xi=xf
!   write(16,*) intnode%parent_fibre%mag%name
!   write(16,*) intnode%parent_fibre%parent_layout%name
!   write(16,*) V3d%x(1:4)
 !  c=>c%next
   intnode=>intnode%next
 enddo
    call gFlushGraphics

 if(associated(p_gino_2)) then
   write(6,*) "node2 final ",node2
   write(6,*) "name ",intnode%parent_fibre%mag%name
   write(6,*) intnode%parent_fibre%parent_layout%name
 endif


 CASE(0) 
 if(flag_map) then    ! Map tracking
  
   rayt(1:6)=rayt(1:6)-my_fix(1:6)
   rayt=mappt*rayt
   rayt(1:6)=rayt(1:6)
    r%r=rayt(1:6)
 
  else
  do ik=1,turns
   if(r%tc/=0) then
    if(.not.orbit_track) then
    do int_n=1,my_ering%t%n
      call TRACK_node_x(intnode,R%R,my_estate)
      if(.not.check_stable) then
    call check_ptc_exceptions(r,my_ering,my_lost_position)

     write(6,200)" Ray Graphed ",r%s 
     write(6,200)" Ray Tracked ",r%r

     write(6,*) " Tracking done ",r%turn_tracked
   return
      endif
!    call TRACK_NODE_SINGLE(intnode,R%R,my_estate,my_ering%CHARGE)
     intnode=>intnode%next
    enddo 
    else 
      
      call ORBIT_TRACK_ONE_TURN(m_orbit_start,R%R)
if(random_colour) then
  	 910 v=ABS(RANF()*360.0)
     if(v>360.0) goto 910
     call gDefineHLS(colour_random, v, 0.5, 1.0)
 my_colour=colour_random
else
  my_colour=r%colour
endif

      if(.not.check_stable.or.R%R(1)>=XBIG/2.d0) then
      
     write(6,200)" Ray Graphed ",r%s 
     write(6,200)" Ray Tracked ",r%r

      write(6,*) " Orbit Tracking done ",r%turn_tracked
      return
      endif
     
    endif
   else
    if(.not.orbit_track) then
     CALL TRACK_probe_X(my_ering,R%R,my_estate,fibre1=MY_START)
     else
         call ORBIT_TRACK_ONE_TURN(m_orbit_start,R%R)

      if(.not.check_stable.or.R%R(1)>=XBIG/2.d0) then
      
     write(6,200)" Ray Graphed ",r%s 
     write(6,200)" Ray Tracked ",r%r

      write(6,*) " Orbit Tracking done ",r%turn_tracked
      return
      endif
   endif
      if(.not.check_stable) then
    call check_ptc_exceptions(r,my_ering,my_lost_position)

     write(6,200)" Ray Graphed ",r%s 
     write(6,200)" Ray Tracked ",r%r

     write(6,*) " Tracking done ",r%turn_tracked
   return
      endif
!    CALL TRACK(my_ering,R%R,MY_START,my_estate)
   endif
  enddo
 endif  ! flag_map


end select

endif   !(i/=0) then
      IF(gEnqKeyState(-3)==1) THEN
       r%interrupt=.true. 
      ENDIF
 if(r%interrupt)   exit
   if(i<=r%nturns)zturns(:,i,1)=r%r
   if(i==r%nturns)  call gmSetWidgetStatus(but1, GSELECTABLE)


if(flag_a) then
 init_bpm(1:6)=r%r(1:6)
 ray(1:c_%nd2)=r%r(1:c_%nd2)-my_fix(1:c_%nd2)
 rayi(1:c_%nd2)=r%r(1:c_%nd2)-my_fix(1:c_%nd2)
 ray=mt*ray
 rayi=invat*rayi
if(i<=nbmp_total) then
 bmp_data(1:6,i)=ray(1:6)
endif

 r%s(1:c_%nd2)=ray(1:c_%nd2)   !  for ploting purposes idiotic
 DO J=1,R%ND
    call egm_scale_draw(plot(5+j),r%s(2*J-1),r%s(2*J),my_colour)
   if(turns==3) call e_gm_draw_rect(my_colour,plot(5+j),0.005,r%s(2*J-1),r%s(2*J))

 !cs(j)=cs(j)+r%s(2*J-1)**2+r%s(2*J)**2
 !rcs(j)=rcs(j)+sqrt(r%s(2*J-1)**2+r%s(2*J)**2)
 cs(j)=cs(j)+ray(2*J-1)**2+ray(2*J)**2
 rcs(j)=rcs(j)+sqrt(ray(2*J-1)**2+ray(2*J)**2)
 if(i==0) then 
   tunes0(j)=atan2(ray(2*J-1),ray(2*J))
   if(tunes0(j)<0.0_dp) tunes0(j)=tunes0(j)+twopi
 else

   tunes(j)=atan2(ray(2*J-1),ray(2*J))
   if(tunes(j)<0.0_dp) tunes(j)=tunes(j)+twopi
   dtunes(j)=(tunes(j)-tunes0(j))/twopi
   tunes0(j)=tunes(j)
   if(dtunes(j)<0.0_dp) dtunes(j)=dtunes(j)+1.0_dp
   total_tunes(j)=total_tunes(j)+dtunes(j)
 endif

 ENDDO
! m=0
!do j=1,3
!m=m+iabs(mres(j))
!enddo
! if(m/=0) then
!   pha=tunes0 
!   phar=(mres(1)*pha(1)+mres(2)*pha(2))/m
!   iperp=rayi(1)
!   if(phar<0.0)phar=phar+twopi
!   call egm_scale_draw(plot(9),phar,iperp,colour_layout)
!   phar=(mres(1)*pha(2)-mres(2)*pha(1))/m
!   iperp=rayi(2)
!   if(phar<0.0)phar=phar+twopi
!   call egm_scale_draw(plot(9),phar,iperp,colour_plot)
! endif

endif

 r%s=r%r

if(around_fix) then
 do j=1,2*R%ND
  r%s(j)=r%s(j)-my_fix(j)
 enddo
endif
if(n_mul>1) then
 if(mod(i,n_mul)==0) then
 DO J=1,R%ND
   call egm_scale_draw(r%WIDGET(J),r%s(2*J-1),r%s(2*J),my_colour)
 ENDDO
 endif
else
 DO J=1,R%ND
  call egm_scale_draw(r%WIDGET(J),r%s(2*J-1),r%s(2*J),my_colour)
 ENDDO
endif
if(mod(i,100*N_mul)==0) THEN
 DO J=1,R%ND
   call gmActivateGraphicsFrame(r%WIDGET(J))
   call gFlushGraphics
 ENDDO
   call gmActivateGraphicsFrame(plot(4))
   call gFlushGraphics
 DO J=1,2    !R%ND
   call gmActivateGraphicsFrame(plot(5+j))
   call gFlushGraphics
 ENDDO
   call gmActivateGraphicsFrame(plot(9))
   call gFlushGraphics

ENDIF

 r%turn_tracked=i
 if(.not.c_%check_stable) exit

!write(18,*) r%r(6),r%r(5)
enddo   !  end turns


!if(flag_layout==1) CALL kill(vers)

DO J=1,R%ND
  call gmActivateGraphicsFrame(r%WIDGET(J))
  call gFlushGraphics
ENDDO
   call gmActivateGraphicsFrame(plot(4))
   call gFlushGraphics
   call gmActivateGraphicsFrame(plot(9))
   call gFlushGraphics
 DO J=1,2   !R%ND
   call gmActivateGraphicsFrame(plot(5+j))
   call gFlushGraphics
 ENDDO

if(around_fix) then
 do j=1,2*R%ND
  r%r(j)=r%r(j)-my_fix(j)
 enddo
endif


if(flag_a) then
 call kill(ai);call kill(mt)
 call kill(inva);call kill(invat)
 c_%check_da=.false.
endif
 if(flag_a.and.turns==1) then
		 nt=i-1
		 if(nt/=0) then
          DO J=1,R%ND
           call egm_scale_draw(plot(5+j),r%s(2*J-1),r%s(2*J),my_colour)
           cs(j)=cs(j)/float(nt)
           rcs(j)=rcs(j)/float(nt)
		   cs(j)=sqrt(abs(cs(j)-rcs(J)**2))
		   total_tunes(j)=total_tunes(j)/float(nt)
          ENDDO
		   if(filename_ap/=' '.and.file_ap>0) then
            write(file_ap,'(i4,4(1x,g16.8),1x,L1,1x,L1)')nt,rcs(1),rcs(2),cs(1),cs(2),r%aperture,r%stable
		   else
		    write(6,*) " File was closed : No Printing"			
		   endif
            write(6,'(i4,4(1x,g16.8),1x,L1,1x,L1)')nt,rcs(1),rcs(2),cs(1),cs(2),r%aperture,r%stable
            write(6,'(a7,3(1x,g16.8))')" tunes from tracking", total_tunes(1:r%nd)
		   endif
 endif

if(flag_map) then
 call kill(mapp);call kill(mappt)
 c_%check_da=.false.
endif

R%IN=.FALSE.
call check_ptc_exceptions(r,my_ering,my_lost_position)

write(6,200)" Ray Graphed ",r%s
write(6,200)" Ray Tracked ",r%r

write(6,*) " Tracking done ",r%turn_tracked
end subroutine track_RING


subroutine compute_only_map(file_map)
implicit none
INTEGER ND2,NPARA
TYPE(DAMAP) ID,m
TYPE(REAL_8) Y(6)
TYPE(NORMALFORM) N
type(pbresonance) f
type(dragtfinn) df
integer f_map
real(dp) cost,sint

character(*) file_map

 MY_FIX=0.D0
 my_fix(5)=my_delta
call FIND_ORBIT_x(my_eRING,my_FIX,my_eSTATE,1.d-5,fibre1=my_start)

!CALL FIND_ORBIT(my_ering,MY_FIX,MY_START,my_estate,1.D-7)

CALL INIT(my_estate,my_order,MY_NP)

CALL ALLOC(ID,m);CALL ALLOC(Y);call alloc(n);call alloc(f);call alloc(df);
id=1

Y=MY_FIX+ID
!CALL TRACK(my_ering,Y,MY_START,my_estate)
    CALL TRACK_probe_X(my_ering,y,+my_estate,fibre1=MY_START)
m=y




f_map=newfile
open(unit=f_map,file=file_map)

call daprint(m,f_map)
f_map=closefile

CALL KILL(ID,m);CALL KILL(Y);
CALL KILL(n);call KILL(f);call KILL(df);

write(6,*) " done "


end subroutine compute_only_map


subroutine sum_res
implicit none
integer no,nd,np,ndpt
integer mf,n1,i
type(real_8) y(6)
type(damap) m,id,nc,n
type(normalform) norm,normt
type(dragtfinn) df
type(taylor) t,u,jx,jy
type(pbfield) hr
type(gmap) g
type(pbresonance) h
type(onelieexponent) oneexp
integer, allocatable :: jc(:)
real(dp) val,prec
real(dp) mr(2),p
real(dp) mu(2),dmu(3)
real(dp) ir,jr 
integer sum

sum=sum_res_val


 MY_FIX=0.D0
 my_fix(5)=my_delta
call FIND_ORBIT_x(my_eRING,my_FIX,only_4d0,1.d-5,fibre1=my_start)


CALL INIT(only_4d0,4,1)
call alloc(m,id,nc,n)
call alloc(norm)
call alloc(normt)
call alloc(df)
call alloc(t,u,jx,jy)
call alloc(h)
call alloc(hr)
call alloc(oneexp)
call alloc(g)
CALL ALLOC(Y)

id=1

Y=MY_FIX+ID

    CALL TRACK_probe_X(my_ering,y,only_4d0,fibre1=MY_START)
m=y

!call compute_only_map("junk.txt")

prec=1.d-10

allocate (jc(c_%nv))


call print(m,6)


df=m
norm=df%linear

!call print(norm%normal%pb,6)

norm%normal%pb%h=norm%normal%pb%h*norm%a_t**(-1)

call print(norm%normal%pb,6)

write(6,*) " original tunes "
write(6,*) norm%tune(1:2)
write(6,*) norm%tune(1)+norm%tune(2)
 
t=zero
u=zero

    call taylor_cycle(norm%normal%pb%h,size=n1)

    do i=1,n1
       call taylor_cycle(norm%normal%pb%h,ii=i,value=val,j=jc)
       
       if(jc(1)+jc(2)==1) then
        u=u+(val.mono.jc)  
        jc(c_%nv)=1
        t=t+(val.mono.jc)  
       endif       
    enddo
call print(t,6)
call print(u,6)
  
norm%normal%pb%h=norm%normal%pb%h-u+t

!call print(norm%normal%pb,6)

norm%normal%pb%ifac=0
id=1
m=texp(norm%normal%pb,id)

norm=m
write(6,*) " Modified  tunes : no coupling "
write(6,*) norm%tune(1:2)
write(6,*) norm%tune(1)+norm%tune(2)
 
df%linear=1
id=df

m=id*m



call kill(norm); call alloc(norm);

norm%nres=2
norm%m(1,1)=1
norm%m(2,1)=sum
norm%m(1,2)=2
norm%m(2,2)=2*sum
mr(1)=1.d0
mr(2)=sum
p=0.5d0*(1+sum)




norm=m

a_res=norm%a_t 
a_resi=norm%a_t**(-1)

write(6,*) norm%tune(1:2)
write(6,*) norm%tune(1)+sum*norm%tune(2)


h=norm%normal%pb
call clean_taylor(h%cos%h,h%cos%h,prec)
call clean_taylor(h%sin%h,h%sin%h,prec)
!call print(h,6)

jx=((1.d0.mono.'2')+(1.d0.mono.'02'))/two
jy=((1.d0.mono.'002')+(1.d0.mono.'0002'))/two

hr%h=(twopi*p/(mr(1)**2+mr(2)**2))*(mr(1)*jx+mr(2)*jy)

n=norm%normal
nc=texp(hr,n)
norm=nc
write(6,*) " rotated  tunes : no coupling "
write(6,*) norm%tune(1:2)
write(6,*) norm%tune(1)+sum*norm%tune(2)

normt=nc
!oneexp=nc
!h=oneexp%pb

normt%normal%pb%H=normt%normal%pb%h*normt%A_t**(-1)
H=normt%normal%pb

call clean_taylor(h%cos%h,h%cos%h,prec)
call clean_taylor(h%sin%h,h%sin%h,prec)
call print(h,6)

g=1
g%v(c_%nv)=1.d0
h%cos%h=h%cos%h.o.g
h%sin%h=h%sin%h.o.g
!call print(h,6)
mu(1)=(h%cos%h.sub.'11')*2.d0
mu(2)=(h%cos%h.sub.'0011')*2.d0
dmu(1)=(h%cos%h.sub.'22')*4.d0
dmu(2)=(h%cos%h.sub.'1111')*4.d0
dmu(3)=(h%cos%h.sub.'0022')*4.d0
if(sum==1) then
 a_str=sqrt((h%cos%h.sub.'1010')**2+(h%sin%h.sub.'1010')**2) * 2.d0
 a_str2=sqrt((h%cos%h.sub.'2020')**2+(h%sin%h.sub.'2020')**2) * 4.d0
 phi_str=atan2((h%sin%h.sub.'1010'),(h%cos%h.sub.'1010'))
 phi_str2=atan2((h%sin%h.sub.'2020'),(h%cos%h.sub.'2020'))
else
 a_str=sqrt((h%cos%h.sub.'1001')**2+(h%sin%h.sub.'1001')**2) * 2.d0
 phi_str=atan2((h%sin%h.sub.'1001'),(h%cos%h.sub.'1001'))
endif
 phi_str=mod(phi_str,twopi)
 if(phi_str<0) phi_str=phi_str+twopi

mur(1)=2*(mu(1)+mu(2))
mur(2)=2*(mu(1)-mu(2))
dmur(1)=dmu(1)+dmu(2)+dmu(3)
dmur(2)=2*(dmu(1)-dmu(3))
dmur(3)=dmu(1)-dmu(2)+dmu(3)

jr=0.d0

call compute_jsol

write(6,*) mur
write(6,*) dmur
write(6,*) sum_res_val
write(6,*) A_str
write(6,*) phi_str
write(6,*) A_str2
write(6,*) phi_str2
write(6,*) Jsol

      call gmSetValueSetting( imur(1), mur(1))     
      call gmSetValueSetting( imur(2), mur(2))     
      call gmSetValueSetting( idmur(1), dmur(1))     
      call gmSetValueSetting( idmur(2), dmur(2))     
      call gmSetValueSetting( idmur(3), dmur(3))     

      call gmSetValueSetting( isum_res_val, sum_res_val)
      call gmSetValueSetting( iA_str, A_str)
      call gmSetValueSetting( iphi_str, phi_str)
      call gmSetValueSetting( ij_perp, j_perp*j_scale)
      call gmSetValueSetting( ijsol(1), jsol(1)*j_scale)
      call gmSetValueSetting( ijsol(2), jsol(2)*j_scale)


call kill(m,id,nc,n)
call kill(norm)
call kill(normt)
call kill(df)
call kill(t,u,jx,jy)
call kill(h)
call kill(hr)
call kill(oneexp)
call kill(g)
CALL kill(Y)

end subroutine sum_res

subroutine compute_jsol
implicit none

if(sum_res_val==1) then
 Jsol(1)=(-mur(1)-a_str)/(2*dmur(1))
 Jsol(2)=(-mur(1)+a_str)/(2*dmur(1))
else
 Jsol(1)=-mur(1)/(2*dmur(3)+a_str)
 Jsol(2)=-mur(1)/(2*dmur(3)-a_str)
endif
      call gmSetValueSetting( ijsol(1), jsol(1)*j_scale)
      call gmSetValueSetting( ijsol(2), jsol(2)*j_scale)


end subroutine compute_jsol


subroutine plot_sum_res(colour,n0)
implicit none
integer i,n0,colour
real(dp) dt,I_psi,psi,k1(2),k2(2),k3(2),k4(2)
real(dp) I_psi1,psi1,phit
real x(2)

   call gmActivateGraphicsFrame(plot(10))

dt=dt_sum_res/mur(1)*twopi
phit=phi_str

I_psi=I_0
psi=psi_0
!phi_str=0
do i=1,n0 
 
 k1(1)= dt* (mur(1)+2*dmur(1)*I_psi + dmur(2)*J_perp +I_psi*A_str*cos(psi-phi_str)/sqrt(I_psi**2-J_perp**2))
 k1(2)=dt*A_str*sin(psi-phi_str)*sqrt(I_psi**2-J_perp**2)
  psi1=psi+half*k1(1)
  I_psi1=I_psi+half*k1(2)
 k2(1)= dt* (mur(1)+2*dmur(1)*I_psi1 + dmur(2)*J_perp +I_psi1*A_str*cos(psi1-phi_str)/sqrt(I_psi1**2-J_perp**2))
 k2(2)=dt*A_str*sin(psi1-phi_str)*sqrt(I_psi1**2-J_perp**2)
  psi1=psi+half*k2(1)
  I_psi1=I_psi+half*k2(2)
 k3(1)= dt* (mur(1)+2*dmur(1)*I_psi1 + dmur(2)*J_perp +I_psi1*A_str*cos(psi1-phi_str)/sqrt(I_psi1**2-J_perp**2))
 k3(2)=dt*A_str*sin(psi1-phi_str)*sqrt(I_psi1**2-J_perp**2)
  psi1=psi+k3(1)
  I_psi1=I_psi+k3(2)
 k4(1)= dt* (mur(1)+2*dmur(1)*I_psi1 + dmur(2)*J_perp +I_psi1*A_str*cos(psi1-phi_str)/sqrt(I_psi1**2-J_perp**2))
 k4(2)=dt*A_str*sin(psi1-phi_str)*sqrt(I_psi1**2-J_perp**2)
   psi=psi+(k1(1)+2*k2(1)+2*k3(1)+k4(1))/6.0_dp
   I_psi=I_psi+(k1(2)+2*k2(2)+2*k3(2)+k4(2))/6.0_dp
 psi=mod(psi,twopi)
 if(psi<0) psi=psi+twopi
 
 x(1)=psi
 x(2)=i_psi*j_scale

! write(6,*) x
  call egm_scale_draw(plot(10),x(1),x(2),colour)
  if(mod(i,20)==0)  call gFlushGraphics
if(stop_track) then
 stop_track=.false.
 exit
endif
enddo 
   call gFlushGraphics
I_0   = I_psi
psi_0 = psi

phi_str=phit

end subroutine plot_sum_res

subroutine use_previous_ray
implicit none
real(dp) x(4),jx,jy,phix,phiy

x=zero
x=matmul(a_resi,previous_ray(1:4))

jx=(x(1)**2+x(2)**2)/2.d0
jy=(x(3)**2+x(4)**2)/2.d0
phix=atan2(-x(2),x(1))
phiy=atan2(-x(4),x(3))

I_0=(jx+jy)/2.d0
j_perp=(jx-jy)/2.d0
psi_0=phix+phiy

call gmSetValueSetting( ij_perp, j_perp*j_scale)
call gmSetValueSetting( iI_0, I_0*j_scale)
call gmSetValueSetting( ipsi_0, psi_0)
      
end subroutine use_previous_ray

subroutine send_to_use_previous_ray
implicit none
real(dp) x(4),jx,jy,phix,phiy


jx= I_0+j_perp
jy= I_0-j_perp
phix= psi_0/2.d0
phiy= psi_0/2.d0  

x(1)=sqrt(2*jx)*cos(phix)
x(2)=-sqrt(2*jx)*sin(phix)
x(3)=sqrt(2*jy)*cos(phiy)
x(4)=-sqrt(2*jy)*sin(phiy)

previous_ray(1:4)=matmul(a_res,x)

write(6,'(a13,4(1x,g21.14))') "ray to track ", previous_ray(1:4)

end subroutine send_to_use_previous_ray


subroutine track_special_one_d(  xr,yr,n0,colour) 
implicit none
real xr,yr,xx,yy
real(dp) x,y,mu,xt,epsi,ja,ray(lnv),mua
integer n0,colour,i
type(damap) id
type(pbfield) h
type(taylor) t
type(dragtfinn) df
type(normalform) normal
ray=0.d0

epsi=-.001d0
mua=.253d0

   call gmActivateGraphicsFrame(plot(10))


mu=twopi*(0.25d0+epsi)

x=xr
y=yr
 do i=1,n0
  xt=cos(mu)*x+sin(mu)*y
   y=cos(mu)*y-sin(mu)*x
  x=xt
  y=y-x**3
  xx=x
  yy=y
  call egm_scale_draw(plot(10),xx,yy,colour)
  if(mod(i,20)==0)  call gFlushGraphics

 enddo
   call gFlushGraphics

call init(4,1,0,0)
call alloc(id)
call alloc(t)
call alloc(df)
call alloc(normal)
call alloc(h)

   id=1
   t=cos(mu)*id%v(1)+sin(mu)*id%v(2)
   id%v(2)=cos(mu)*id%v(2)-sin(mu)*id%v(1)
   id%v(1)=t
   id%v(2)=id%v(2)-id%v(1)**3
   
   df=id
   
   normal=id
   
   call print(normal%dhdj%v(2),6)
   
   ja=(mua-(normal%dhdj%v(2).sub.'0'))/2.d0/(normal%dhdj%v(2).sub.'11')
   
   Write(6,*) "2*ja, x_a ", 2*ja,sqrt(abs(2*ja))
   
   h%h=-twopi*mua/2.d0*((1.d0.mono.'2')+(1.d0.mono.'02'))
   
   df=id
   df%linear=1
   df%linear=exp(h,df%linear)
   id=df
      normal=id
   call print(normal%dhdj%v(2),6)
 id=normal%a_t**(-1)
 
 x=xr
y=yr
 do i=1,n0
  xt=cos(mu)*x+sin(mu)*y
   y=cos(mu)*y-sin(mu)*x
  x=xt
  y=y-x**3
  ray(1)=x
  ray(2)=y
  ray=id*ray(1:2)
  xx=ray(1)
  yy=ray(2)
  call egm_scale_draw(plot(10),xx,yy,gred)
  if(mod(i,20)==0)  call gFlushGraphics

 enddo
   call gFlushGraphics

   call kill(id)
   call kill(t)
   call kill(df)
   call kill(normal)


end subroutine track_special_one_d  

end module Zb_physics


